/*
  姿态九轴传感器例程：根据四元数计算欧拉角，使用madgwick算法
    Quaternion包含四元数数据，分量命名为 qW qX qY qZ
  并根据匿名上位机V7的数据协议将数据通过串口上传到上位机软件，实现可视化数据观察

  上位机软件官网：https://www.anotc.com/
  调用library: https://github.com/LiquidCGS/FastIMU
*/

#include <Wire.h>
#include "FastIMU.h"
#include "Madgwick.h"

// #define QUAT  // 定义则四元数据显示，注释则欧拉角数据显示

#define IMU_ADDRESS 0x6A     // 陀螺仪及加速度传感器地址(IMU address)，默认0x6A，无需更改
#define PERFORM_CALIBRATION  // 注释以禁用启动校准-Comment to disable startup calibration
QMI8658_QMC5883L IMU;        // Change to the name of any supported IMU!

calData calib = { 0 };  //Calibration data
AccelData IMUAccel;     //Sensor data
GyroData IMUGyro;
MagData IMUMag;
Madgwick filter;

uint8_t RxBuff[1];

void setup() {
  Wire.begin();
  Wire.setClock(400000);  //400khz clock

  Serial.begin(115200);
  while (!Serial) { ; }

  int err = IMU.init(calib, IMU_ADDRESS);
  if (err != 0) {
    Serial.print("Error initializing IMU: ");
    Serial.println(err);
    while (true) {
      ;
    }
  }

  if (err != 0) {
    Serial.print("Error Setting range: ");
    Serial.println(err);
    while (true) {
      ;
    }
  }

// 传感器校准
#ifdef PERFORM_CALIBRATION
  Serial.println("FastIMU Calibrated Quaternion example");
  if (IMU.hasMagnetometer()) {
    delay(1000);
    Serial.println("Move IMU in figure 8 pattern until done.");
    delay(3000);
    IMU.calibrateMag(&calib);
    Serial.println("Magnetic calibration done!");
  } else {
    delay(1000);
  }
  Serial.println("Keep IMU level.");
  delay(5000);
  IMU.calibrateAccelGyro(&calib);
  Serial.println("Calibration done!");
  Serial.println("Accel biases X/Y/Z: ");
  Serial.print(calib.accelBias[0]);
  Serial.print(", ");
  Serial.print(calib.accelBias[1]);
  Serial.print(", ");
  Serial.println(calib.accelBias[2]);
  Serial.println("Gyro biases X/Y/Z: ");
  Serial.print(calib.gyroBias[0]);
  Serial.print(", ");
  Serial.print(calib.gyroBias[1]);
  Serial.print(", ");
  Serial.println(calib.gyroBias[2]);
  if (IMU.hasMagnetometer()) {
    Serial.println("Mag biases X/Y/Z: ");
    Serial.print(calib.magBias[0]);
    Serial.print(", ");
    Serial.print(calib.magBias[1]);
    Serial.print(", ");
    Serial.println(calib.magBias[2]);
    Serial.println("Mag Scale X/Y/Z: ");
    Serial.print(calib.magScale[0]);
    Serial.print(", ");
    Serial.print(calib.magScale[1]);
    Serial.print(", ");
    Serial.println(calib.magScale[2]);
  }
  delay(5000);
  IMU.init(calib, IMU_ADDRESS);

  filter.begin(0.2f);
#endif
}


void Pack_Data(uint8_t fun, uint8_t *data, uint8_t len) {
  uint8_t send_buf[32];
  uint8_t i;
  if (len > 28) return;  //最多28字节数据
  send_buf[len + 4] = 0;
  send_buf[0] = 0XAA;                                   //帧头
  send_buf[1] = 0xAA;                                   // 目标地址
  send_buf[2] = fun;                                    //功能字
  send_buf[3] = len;                                    //数据长度
  for (i = 0; i < len; i++) send_buf[4 + i] = data[i];  //复制数据
  uint8_t sumcheck = 0;
  uint8_t addcheck = 0;
  for (i = 0; i < (len + 4); i++) {
    sumcheck += send_buf[i];  //从帧头开始，对每一字节进行求和，直到DATA区结束
    addcheck += sumcheck;     //每一字节的求和操作，进行一次sumcheck的累加
  }
  send_buf[len + 4] = sumcheck;  //和校验
  send_buf[len + 5] = addcheck;  //附加校验
  for (i = 0; i <= len + 5; i++) Serial.write(send_buf[i]);

  // //如果计算出的sumcheck和addcheck和接收到的check数据相等，代表校验通过，反之数据有误
  // if (sumcheck == data_buf[data_buf[3] + 4] && addcheck == data_buf[data_buf[3] + 5]) {
  //   // return true;  //校验通过
  // } else
  // // return false;  //校验失败
}
// 欧拉角 数据打包发送
void Send_SensorData(int16_t dat1, int16_t dat2, int16_t dat3, uint8_t dat4) {
  uint8_t tbuf[18];
  // 欧拉角格式
  tbuf[0] = dat1 & 0XFF;
  tbuf[1] = (dat1 >> 8) & 0XFF;
  tbuf[2] = dat2 & 0XFF;
  tbuf[3] = (dat2 >> 8) & 0XFF;
  tbuf[4] = dat3 & 0XFF;
  tbuf[5] = (dat3 >> 8) & 0XFF;
  tbuf[6] = dat4 & 0XFF;
  Pack_Data(0x03, tbuf, 7);
}
// 四元数据 数据打包发送
void Send_SensorData_quat(int16_t dat1, int16_t dat2, int16_t dat3, int16_t dat4, uint8_t dat5) {
  uint8_t tbuf[18];
  tbuf[0] = dat1 & 0XFF;
  tbuf[1] = (dat1 >> 8) & 0XFF;
  tbuf[2] = dat2 & 0XFF;
  tbuf[3] = (dat2 >> 8) & 0XFF;
  tbuf[4] = dat3 & 0XFF;
  tbuf[5] = (dat3 >> 8) & 0XFF;
  tbuf[6] = dat4 & 0XFF;
  tbuf[7] = (dat4 >> 8) & 0XFF;
  tbuf[8] = dat5 & 0XFF;
  Pack_Data(0x04, tbuf, 9);
}

void loop() {
  IMU.update();
  IMU.getAccel(&IMUAccel);
  IMU.getGyro(&IMUGyro);
  if (IMU.hasMagnetometer()) {
    IMU.getMag(&IMUMag);
    filter.update(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ, IMUMag.magX, IMUMag.magY, IMUMag.magZ);
  } else {
    filter.updateIMU(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ);
  }

  // 获取四元数
  float q0 = filter.getQuatW();
  float q1 = filter.getQuatX();
  float q2 = filter.getQuatY();
  float q3 = filter.getQuatZ();
  // Serial.print("Q0: ");
  // Serial.print(q0);
  // Serial.print("\tQ1: ");
  // Serial.print(q1);
  // Serial.print("\tQ2: ");
  // Serial.print(q2);
  // Serial.print("\tQ3: ");
  // Serial.println(q3);

#ifdef QUAT  // 四元数据显示
  // 发送四元数据（数据需要放大10000倍）至 匿名上位机V7.2.5
  Send_SensorData_quat(q0 * 10000, q1 * 10000, q2 * 10000, q3 * 10000, 0);

#else  // 欧拉角数据显示

  // 避免重复计算
  // float q0q0 = q0 * q0;
  float q1q1 = q1 * q1;
  float q2q2 = q2 * q2;
  float q3q3 = q3 * q3;

  // 计算欧拉角（roll, pitch, yaw） - 注意：这里假设您想要的是以弧度为单位的值
  float roll = atan2(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1q1 + q2q2));
  float pitch = -asin(2.0f * (q0 * q2 - q3 * q1));
  float yaw = -atan2(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2q2 + q3q3));

  // 如果需要将弧度转换为度数，可以使用以下公式：
  roll = fmod(roll * 180.0f / PI, 360.0f);
  pitch = fmod(pitch * 180.0f / PI, 360.0f);
  yaw = fmod(yaw * 180.0f / PI, 360.0f);

  // 打印欧拉角
  // Serial.print("Roll: ");
  // Serial.print(roll);
  // Serial.print("\tPitch: ");
  // Serial.print(pitch);
  // Serial.print("\tYaw: ");
  // Serial.println(yaw);

  // 发送欧拉角数据（数据需要放大100倍）至 匿名上位机V7.2.5
  Send_SensorData(roll * 100, pitch * 100, yaw * 100, 0);

#endif
  delay(50);
}
